#include "AutoPID.h"
#include <math.h>
#include <stdlib.h>

#define constrain(x, a, b)     (x < a ? a : x > b ? b : x)

int millis(void);

void AutoPID_AutoPID(struct AutoPID *ap, float *input, float *setpoint, float *output, float outputMin, float outputMax,
                     float Kp, float Ki, float Kd)
{
    ap->_input = input;
    ap->_setpoint = setpoint;
    ap->_output = output;
    ap->_outputMin = outputMin;
    ap->_outputMax = outputMax;
    AutoPID_setGains(ap, Kp, Ki, Kd);
    ap->_timeStep = 1000;
}

void AutoPID_setGains(struct AutoPID *ap, float Kp, float Ki, float Kd)
{
    ap->_Kp = Kp;
    ap->_Ki = Ki;
    ap->_Kd = Kd;
} // AutoPID::setControllerParam

void AutoPID_setBangBang(struct AutoPID *ap, float bangOn, float bangOff)
{
    ap->_bangOn = bangOn;
    ap->_bangOff = bangOff;
} // void AutoPID::setBangBang

void AutoPID_setBangBangRange(struct AutoPID *ap, float bangRange)
{
    AutoPID_setBangBang(ap, bangRange, bangRange);
} // void AutoPID::setBangBang

void AutoPID_setOutputRange(struct AutoPID *ap, float outputMin, float outputMax)
{
    ap->_outputMin = outputMin;
    ap->_outputMax = outputMax;
} // void AutoPID::setOutputRange

void AutoPID_setTimeStep(struct AutoPID *ap, unsigned long timeStep)
{
    ap->_timeStep = timeStep;
}

bool AutoPID_atSetPoint(struct AutoPID *ap, float threshold)
{
    return abs(*(ap->_setpoint) - *(ap->_input)) <= threshold;
} // bool AutoPID::atSetPoint

void AutoPID_run(struct AutoPID *ap)
{
    if (ap->_stopped)
    {
        ap->_stopped = false;
        AutoPID_reset(ap);
    }
    // if bang thresholds are defined and we're outside of them, use bang-bang control
    if (ap->_bangOn && ((*ap->_setpoint - *ap->_input) > ap->_bangOn))
    {
        *ap->_output = ap->_outputMax;
        ap->_lastStep = millis();
    }
    else if (ap->_bangOff && ((*ap->_input - *ap->_setpoint) > ap->_bangOff))
    {
        *ap->_output = ap->_outputMin;
        ap->_lastStep = millis();
    }
    else
    {                                             // otherwise use PID control
        unsigned long _dT = millis() - ap->_lastStep; // calculate time since last update
        if (_dT >= ap->_timeStep)
        { // if long enough, do PID calculations
            ap->_lastStep = millis();
            float _error = *ap->_setpoint - *ap->_input;
            ap->_integral += (_error + ap->_previousError) / 2 * _dT / 1000.0; // Riemann sum integral
            //_integral = constrain(_integral, _outputMin/_Ki, _outputMax/_Ki);
            float _dError = (_error - ap->_previousError) / _dT / 1000.0; // derivative
            ap->_previousError = _error;
            float PID = (ap->_Kp * _error) + (ap->_Ki * ap->_integral) + (ap->_Kd * _dError);
            //*_output = _outputMin + (constrain(PID, 0, 1) * (_outputMax - _outputMin));
            *ap->_output = constrain(PID, ap->_outputMin, ap->_outputMax);
        }
    }
} // void AutoPID::run

void AutoPID_stop(struct AutoPID *ap)
{
    ap->_stopped = true;
    AutoPID_reset(ap);
}

void AutoPID_reset(struct AutoPID *ap)
{
    ap->_lastStep = millis();
    ap->_integral = 0;
    ap->_previousError = 0;
}

bool AutoPID_isStopped(struct AutoPID *ap)
{
    return ap->_stopped;
}

float AutoPID_getIntegral(struct AutoPID *ap)
{
    return ap->_integral;
}

void AutoPID_setIntegral(struct AutoPID *ap,float integral)
{
    ap->_integral = integral;
}


void AutoPIDRelay_run(struct AutoPIDRelay *apr) 
{
    AutoPID_run(&apr->ap);
    while ((millis() - apr->_lastPulseTime) > apr->_pulseWidth) 
        apr->_lastPulseTime += apr->_pulseWidth;
    *apr->_relayState = ((millis() - apr->_lastPulseTime) < (apr->_pulseValue * apr->_pulseWidth));
}

float AutoPIDRelay_getPulseValue(struct AutoPIDRelay *apr)
{
    return (AutoPID_isStopped(&apr->ap) ? 0 : apr->_pulseValue);
}